﻿using EtherCAT.NET.Infrastructure;
using Jodell.Transport.Ethercat.Esi;
using log4net;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Reflection;
using System.Runtime.InteropServices;
using System.Threading;
using System.Timers;

namespace Jodell.Transport.Ethercat
{

    public class EcSlaveConfigEventArgs : EventArgs
    {
        public bool IsCancel { get; set; }
        public EcSlave Slave { get; set; }
    }

    public interface IEcMaster
    {
        event EventHandler<EcSlaveConfigEventArgs> ConfigSlaveEsi;
        IntPtr Ptr_Context { get; }
        bool InOp { get; set; }
        bool IsRunning { get; }
        List<EcSlave> Slaves { get; }

        List<EcAdapter> FindAdapters();

        void Start(string ifname);
        void Stop();
        void Sync();

        void EnterReadLock();
        void ExitReadLock();
        void EnterWriteLock();
        void ExitWriteLock();
        bool TryEnterReadLock(int millisecondsTimeout);
        bool TryEnterWriteLock(int millisecondsTimeout);

        int GetSlaveState(int slaveNo);
        T ReadPdo<T>(UInt16 slave, UInt16 index, byte subindex) where T : unmanaged;
        void WritePdo<T>(UInt16 slave, UInt16 index, byte subindex, T data) where T : unmanaged;
        T ReadSdo<T>(UInt16 slave, UInt16 index, byte subindex, bool CA, int timeout = 700000) where T : unmanaged;
        void WriteSdo<T>(UInt16 slave, UInt16 index, byte subindex, T value, bool CA, int timeout = 700000) where T : unmanaged;
        double ReadSdo(UInt16 slave, UInt16 index, byte subindex, string datatype, int timeout = 70000);
        int ecx_SDOread(ushort slave, ushort index, byte subindex, bool CA, ref int psize, IntPtr p, int timeout);
    }


    public class EcMaster : IEcMaster
    {
        ILog _log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
        public IntPtr Ptr_Context { get; private set; }
        IntPtr SlaveStatesPtr = Marshal.AllocHGlobal(128 * 2);
        public EcMaster()
        {
            Debug.WriteLine("EcMaster");

            syncTimer.Interval = 1;
            syncTimer.Elapsed += SyncTimer_Elapsed;

            stateCheckTimer.Interval = 10;
            stateCheckTimer.Elapsed += StateCheckTimer_Elapsed;
        }
        private ReaderWriterLockSlim lockObj = new ReaderWriterLockSlim();

        public void EnterReadLock()
        {
            lockObj.EnterReadLock();
        }
        public void ExitReadLock()
        {
            lockObj.ExitReadLock();
        }
        public void EnterWriteLock()
        {
            lockObj.EnterWriteLock();
        }
        public void ExitWriteLock()
        {
            lockObj.ExitWriteLock();
        }
        public bool TryEnterReadLock(int millisecondsTimeout)
        {
            return lockObj.TryEnterReadLock(millisecondsTimeout);
        }
        public bool TryEnterWriteLock(int millisecondsTimeout)
        {
            return lockObj.TryEnterWriteLock(millisecondsTimeout);
        }

        private void StateCheckTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (this.IsRunning && this.Slaves.All(x => x.RequestOp))//手动切换状态   
            {
                if (this.TryEnterWriteLock(1))
                {
                    EcMain.ecatcheck();
                    this.ExitWriteLock();
                }
            }
        }

        private void SyncTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (this.Slaves.Count > 0 && !this.Slaves.Any(x => x.RequestState == 3))//在boot模式下，不发送同步数据  //this.Slaves.All(x => x.RequestOp)
            {
                if (this.TryEnterWriteLock(1))
                {
                    EcMain.ecx_send_processdata(this.Ptr_Context);
                    EcMain.ecx_receive_processdata(this.Ptr_Context, Constants.EC_TIMEOUTRET);
                    this.ExitWriteLock();
                }
            }
        }

        public List<EcAdapter> FindAdapters()
        {
            List<EcAdapter> adapters = new List<EcAdapter>();
            IntPtr ptr = EcMain.ec_find_adapters();
            IntPtr ptr_tmp = ptr;
            while (ptr_tmp != IntPtr.Zero)
            {
                ec_adapter adp = Marshal.PtrToStructure<ec_adapter>(ptr_tmp);
                adapters.Add(new EcAdapter()
                {
                    Name = adp.name,
                    Description = adp.desc,
                });
                ptr_tmp = adp.next;
            }
            EcMain.ec_free_adapters(ptr);
            return adapters;
        }
        public string IfName { get; private set; }

        private bool inOp;

        public bool InOp
        {
            get { return inOp; }
            set { inOp = value; }
        }

        private System.Timers.Timer syncTimer = new System.Timers.Timer();
        private System.Timers.Timer stateCheckTimer = new System.Timers.Timer();
        public List<EcSlave> Slaves { get; private set; } = new List<EcSlave>();



        public event EventHandler<EcSlaveConfigEventArgs> ConfigSlaveEsi;

        public void Start(string ifname)
        {
            try
            {
                this.Slaves.Clear();

                IfName = ifname;

                var ret = EcMain.ScanDevice(ifname);
                if (ret < 0)
                {
                    throw new SoemException(SoemException.ERROR_SLAVE_NOT_FOUND);
                }
                Slaves.Clear();
                int count = 0;
                this.Ptr_Context = EcMain.get_ec_context();
                var slaveCount = EcMain.get_ec_slavecount();
                IntPtr slaves = Marshal.AllocHGlobal(slaveCount * Marshal.SizeOf(typeof(EcSlaveInfo)));
                EcMain.GetSlaves(slaves, ref count);
                IntPtr intPtr = slaves;
                for (int i = 0; i < count; i++)
                {
                    var slave = Marshal.PtrToStructure<EcSlaveInfo>(intPtr);

                    if (slave.Manufacturer != 0x00010100)//只允许钧舵设备
                    {
                        _log.WarnFormat("发现未知厂家设备 ProductCode：0x{0:X8}", slave.ProductCode);
                        continue;
                    }

                    var _slave = new EcSlave
                    {
                        Inputs = slave.Inputs,
                        Outputs = slave.Outputs,
                        Csa = slave.Csa,
                        Manufacturer = slave.Manufacturer,
                        Name = slave.Name,
                        OldCsa = slave.OldCsa,
                        ProductCode = slave.ProductCode,
                        Revision = slave.Revision,
                    };
                    var arg = new EcSlaveConfigEventArgs() { Slave = _slave, IsCancel = false };
                    ConfigSlaveEsi?.Invoke(this, arg);
                    if (!arg.IsCancel)
                    {
                        _slave.RequestOp = true;
                        Slaves.Add(_slave);
                    }
                    intPtr = IntPtr.Add(intPtr, Marshal.SizeOf(typeof(EcSlaveInfo)));
                }
                InOp = true;
                IsRunning = true;
                syncTimer.Start();
                stateCheckTimer.Start();
            }
            catch
            {
                throw;
            }
        }

        public void Stop()
        {
            this.EnterWriteLock();
            InOp = false;
            IsRunning = false;
            syncTimer.Stop();
            stateCheckTimer.Stop();
            if (this.Ptr_Context != IntPtr.Zero)
            {
                //EcMain.Stop();
                EcMain.ec_close();
                this.Ptr_Context = IntPtr.Zero;
            }
            this.ExitWriteLock();
        }

        public bool IsRunning { get; private set; }

        public void Sync()
        {
            EcMain.ecx_send_processdata(this.Ptr_Context);
            EcMain.ecx_receive_processdata(this.Ptr_Context, Constants.EC_TIMEOUTRET);
        }
        public int GetSlaveState(int slaveNo)
        {
            EnterReadLock();
            var state = EcMain.get_state(slaveNo);
            ExitReadLock();
            return state;
        }
        public T ReadPdo<T>(UInt16 slave, UInt16 index, byte subindex) where T : unmanaged
        {
            this.EnterReadLock();
            T ret = default(T);
            if (index != 0x7000 || index != 0x6000)
            {
                throw new SoemException(SoemException.ERROR_PDO_INDEX);
            }
            var _slave = this.Slaves[slave];
            if (index == 0x6000)
            {
                unsafe
                {
                    byte* ptr = (byte*)_slave.Inputs;
                    ptr += _slave.RxPdo.First(x => x.SubIdx == subindex).BitOffs / 8;
                    T* _ptr = (T*)ptr;
                    ret = *_ptr;
                }
            }
            else
            {
                unsafe
                {
                    byte* ptr = (byte*)_slave.Outputs;
                    ptr += _slave.RxPdo.First(x => x.SubIdx == subindex).BitOffs / 8;
                    T* _ptr = (T*)ptr;
                    ret = *_ptr;
                }
            }
            this.ExitReadLock();
            return ret;
        }
        public void WritePdo<T>(UInt16 slave, UInt16 index, byte subindex, T data) where T : unmanaged
        {
            if (index != 0x7000 && index != 0x6000)
            {
                throw new SoemException(SoemException.ERROR_PDO_INDEX);
            }
            var _slave = this.Slaves[slave];
            IntPtr intPtr = IntPtr.Zero;
            int bitOffs = 0;
            if (index == 0x6000)
            {
                intPtr = _slave.Inputs;
                bitOffs = _slave.RxPdo.First(x => x.SubIdx == subindex).BitOffs;
            }
            else
            {
                intPtr = _slave.Outputs;
                bitOffs = _slave.TxPdo.First(x => x.SubIdx == subindex).BitOffs;
            }
            unsafe
            {
                byte* ptr = (byte*)intPtr;
                ptr += bitOffs / 8;
                T* _ptr = (T*)ptr;
                this.EnterWriteLock();
                *_ptr = data;
                Sync();
                this.ExitWriteLock();
            }
        }
        /// <summary>
        /// 
        /// </summary>
        /// <typeparam name="T"></typeparam>
        /// <param name="slave"></param>
        /// <param name="index"></param>
        /// <param name="subindex"></param>
        /// <param name="timeout">超时时间 μs 700000</param>
        /// <returns></returns>
        public T ReadSdo<T>(UInt16 slave, UInt16 index, byte subindex, bool CA, int timeout = 700000) where T : unmanaged
        {
            if (!this.IsRunning)
            {
                return default(T);
            }

            T ret = default(T);
            int psize = 0;
            IntPtr ptr = Marshal.AllocHGlobal(128);
            this.EnterReadLock();
            var wkc = EcMain.ecx_SDOread(this.Ptr_Context, slave, index, subindex, CA, ref psize, ptr, timeout);
            this.ExitReadLock();

            if (wkc > 0)
            {
                unsafe
                {
                    ret = *(T*)ptr.ToPointer();
                }
                Marshal.FreeHGlobal(ptr);
            }
            else
            {
                throw new SoemException(SoemException.ERROR_SDO_TIMEOUT);
            }
            return ret;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <typeparam name="T"></typeparam>
        /// <param name="slave"></param>
        /// <param name="index"></param>
        /// <param name="subindex"></param>
        /// <param name="value"></param>
        /// <param name="timeout"></param>
        /// <exception cref="SoemException"></exception>
        public void WriteSdo<T>(UInt16 slave, UInt16 index, byte subindex, T value, bool CA, int timeout = 700000) where T : unmanaged
        {
            int psize = 0;
            IntPtr ptr = Marshal.AllocHGlobal(128);
            unsafe
            {
                psize = sizeof(T);
                *(T*)ptr.ToPointer() = value;
            }
            this.EnterReadLock();
            var wkc = EcMain.ecx_SDOwrite(this.Ptr_Context, slave, index, subindex, CA, psize, ptr, timeout);
            this.ExitReadLock();
            Marshal.FreeHGlobal(ptr);
            if (wkc <= 0)
            {
                throw new SoemException(SoemException.ERROR_SDO_TIMEOUT);
            }
        }


        public double ReadSdo(UInt16 slave, UInt16 index, byte subindex, string datatype, int timeout = 70000)
        {
            if (!this.IsRunning)
            {
                return 0;
            }
            int psize = 8;
            double data = 0;
            IntPtr ptr = Marshal.AllocHGlobal(8);
            this.EnterReadLock();
            var wkc = EcMain.ecx_SDOread(this.Ptr_Context, slave, index, subindex, false, ref psize, ptr, timeout);
            this.ExitReadLock();
            if (wkc > 0)
            {
                unsafe
                {
                    switch (datatype)
                    {
                        case "USINT": data = *(byte*)ptr.ToPointer(); break;
                        case "SINT": data = *(sbyte*)ptr.ToPointer(); break;
                        case "UINT": data = *(UInt16*)ptr.ToPointer(); break;
                        case "INT": data = *(Int16*)ptr.ToPointer(); break;
                        case "UDINT": data = *(UInt32*)ptr.ToPointer(); break;
                        case "DINT": data = *(Int32*)ptr.ToPointer(); break;
                        case "REAL": data = *(float*)ptr.ToPointer(); break;
                    }
                }
                Marshal.FreeHGlobal(ptr);
            }
            else
            {
                Marshal.FreeHGlobal(ptr);
                throw new SoemException(SoemException.ERROR_SDO_TIMEOUT);
            }
            return data;
        }

        public int ecx_SDOread(ushort slave, ushort index, byte subindex, bool CA, ref int psize, IntPtr p, int timeout)
        {
            int wck = -1;
            this.EnterReadLock();
            if (this.IsRunning)
            {
                wck = EcMain.ecx_SDOread(this.Ptr_Context, slave, index, subindex, CA, ref psize, p, timeout);
            }
            this.ExitReadLock();
            return wck;
        }
    }
}
